function [betaFMM, dOrth, dNorm] = dcmdiff(CHatFM, CFM, firstOrdApprox)
%DCMDIFF 计算方向余弦矩阵的差值
%
% Input Arguments:
% # CHatFM: 3*3*m矩阵，比较方向余弦矩阵
% # CFM: 3*3*m矩阵，参考方向余弦矩阵
% # firstOrdApprox: 逻辑标量，true表示使用一阶近似算法，否则使用精确算法，默认为true
%
% Output Arguments:
% # betaFMM: m*3向量，比较值相对于参考值的姿态差值，设CHatFM=CFMHat，则beta_FM^M=alpha_MF^M=theta_MMHat^M为MHat相对于M的旋转矢量（在M/MHat系下的投影），单位为rad
% # dOrth: m*3向量，分别为方向余弦矩阵第1、2行，第1、3行，第2、3行向量实际夹角与90度之差（当参考值为正交矩阵时且phi为0时），单位为rad
% # dNorm: m*3向量，分别为方向余弦矩阵第1行，第2行，第3行向量长度平方与1之差的一半（当参考值为正交矩阵时且phi为0时），无单位
%
% Assumptions and Limitations:
% # 本函数假设CFM为正交矩阵，在精确算法下，也假设CHatFM为正交矩阵，此时dOrth与dNorm均为NaN
% # 一阶近似算法在姿态差较大时精度较低，仅供参考
%
% References:
% # 《应用导航算法工程基础》方向余弦矩阵的非正交归一化误差、方向余弦矩阵的失准角误差

if nargin < 3
    firstOrdApprox = true;
end

n = size(CFM, 3);
dOrth = NaN(n, 3);
dNorm = NaN(n, 3);
if firstOrdApprox
    betaFMM = NaN(n, 3);
    for i=1:n
        E = CHatFM(:, :, i) * CFM(:, :, i)' - eye(3);
        ESym = (E + E') / 2;
        ESkSym = (E - E') / 2;
        betaFMM(i, :) = [ESkSym(2, 3) -ESkSym(1, 3) ESkSym(1, 2)];
        dOrth(i, :) = 2 * [ESym(1, 2) ESym(1, 3) ESym(2, 3)];
        dNorm(i, :) = [ESym(1, 1) ESym(2, 2) ESym(3, 3)];
    end
else
    if n == 1
        CMHatM = CFM * CHatFM';
    else
        CMHatM = binopalonglastdim(CFM, transpose_nd(CHatFM));
    end
    betaFMM = dcm2rv(CMHatM);
end
end